#include <iostream>
#include <pthread.h>

#include "Sys3D.h"
#include "Quaternion.h"
#include "Rotation.h"

#include "Timer.h"

using namespace RSIM;

int main (int argc, const char * argv[])
{
        const double& q = 1.0;
        const double& u = 1.0;
        
        const int& nbodies = 2;
        const int& nproc = 1;
        const int& niter = 1;
        
        RSIM::Sys3D mbs3D(nbodies,nproc);
        mbs3D.initDCANodesForDCA();        
        mbs3D.setQ(q);
        mbs3D.setU(u);        
        mbs3D.updRotationMatrix();
        mbs3D.updAngularVelocity();

        for(int i=0;i<niter;++i){
                mbs3D.n_formDCAHandleEquations_Opt(0,nbodies-1);
                mbs3D.n_calcDCAHandleEquationsUsingDCA(0,nbodies-1,0,true);
        }
        
        mbs3D.Asm_[0].dca_.displayH1();
        mbs3D.Asm_[0].dca_.displayH2();
//         mbs3D.solveSystemHandleEquationsCase_OpControl();
//         cout<<mbs3D.iLambda_<<endl;
//         cout<<mbs3D.pe_<<endl;
        
        return 0;
}